#include "occlusion_collision_warning.h"
#include "../remote_vehicle.h"
#include "../utils.h"
#include "../world.h"

#include "interface/msg/event.hpp"

using namespace decision::adas;

OcclusionCollisionWarning::OcclusionCollisionWarning(World& world, double fps)
    : AdasApplication(world, "OcCW", fps)
{
}

void OcclusionCollisionWarning::detect()
{
    RCLCPP_INFO(logger_, "OcclusionCollisionWarning do detect");

    auto neighbers = this_world_.get_neighbers();
    if (!neighbers) {
        RCLCPP_INFO(logger_, "OcclusionCollisionWarning No Neighbers!");
        return;
    }

    auto now = muduo::Timestamp::now().microSecondsSinceEpoch();

    for (auto& n : *neighbers) {
        const auto& neighber = n.second;
        auto ttc = neighber->get_ttc();
        auto predicted_min_distance = neighber->get_predicted_min_distance();
        if (std::isnan(ttc) || ttc > 15 || ttc < 0 || predicted_min_distance > 20) {
            continue;
        }
        if (neighber->x() < -2 && neighber->y() > 2) {
            continue;
        }

        interface::msg::Event event;

        event.header.stamp.sec = now / 1000 / 1000;
        event.header.stamp.nanosec = (now % 1000) * 1000 * 1000;
        event.header.frame_id = "ocw";
        event.type = "ocw";

        if (ttc < 1) {
            event.level = 0;
        } else if (ttc < 2) {

            event.level = 1;
        } else if (ttc < 5) {
            event.level = 2;
        }

        event.vehicles.emplace_back();
        this_world_.report(event);
    }
}
